#ifndef QUATERNION_H
#define QUATERNION_H

#include "Math.h"
#include "Matrix.h"
#include "Vec3.h"

class Quaternion
{
public:
	Quaternion() : x(0), y(0), z(0), s(1) {}
	Quaternion(const Quaternion& aRef) : x(aRef.x), y(aRef.y), z(aRef.z), s(aRef.s) {}
	Quaternion(const Vec3& aAxe, float aAngle) { Set(aAxe, aAngle); }

	inline const Quaternion& operator=(const Quaternion& aRef);

	inline void Set(const Vec3& aAxe, float aAngle);
	inline void Reset() { s = 1; x = 0; y = 0; z = 0; }

	inline void Mult(const Quaternion& aQuatA, const Quaternion& aQuatB);
	inline void SelfMult(const Quaternion& aRef);
	//static inline Mult(const Quaternion& aQuatA, const Quaternion& aQuatB, Quaternion& aQuatOut);
	inline void Conjugate(Quaternion& aQuatConj) const;
	inline void Conjugate();

	inline void SetTransformationMatrix(float aMatrix[16]) const;
	inline void SetTransformationMatrix3(float aMatrix[9]) const;

	inline void Transform(const Vec3& aIn, Vec3& aOut) const;

	inline void Slerp(const Quaternion& aFrom, const Quaternion& aTo, float aT);

	inline void GetAxeAndAngle(Vec3& aAxe, float& aAngle);

private:
	Quaternion(float aX, float aY, float aZ, float aS) : x(aX), y(aY), z(aZ), s(aS) {}
	inline void Normalize();

public:
	float x;
	float y;
	float z;
	float s;
};

inline const Quaternion& Quaternion::operator=(const Quaternion& aRef)
{ 
	x = aRef.x;
	y = aRef.y;
	z = aRef.z;
	s = aRef.s;

	return *this;
}

inline void Quaternion::Set(const Vec3& aAxe, float aAngle)
{
	aAngle *= 0.5f;
	float tLength = aAxe.Magnitude();
	float tSinus = Math::sinf(aAngle) / tLength;

	x = aAxe.X() * tSinus;
	y = aAxe.Y() * tSinus;
	z = aAxe.Z() * tSinus;
	s = Math::cosf(aAngle);
}

inline void Quaternion::Mult(const Quaternion& aQuatA, const Quaternion& aQuatB)
{
	x = aQuatA.s * aQuatB.x + aQuatA.x * aQuatB.s + aQuatA.y * aQuatB.z - aQuatA.z * aQuatB.y;
	y = aQuatA.s * aQuatB.y + aQuatA.y * aQuatB.s + aQuatA.z * aQuatB.x - aQuatA.x * aQuatB.z;
	z = aQuatA.s * aQuatB.z + aQuatA.z * aQuatB.s + aQuatA.x * aQuatB.y - aQuatA.y * aQuatB.x;
	s = aQuatA.s * aQuatB.s - aQuatA.x * aQuatB.x - aQuatA.y * aQuatB.y - aQuatA.z * aQuatB.z;

	Normalize();
}

inline void Quaternion::SelfMult(const Quaternion& aRef)
{
	float tx = x, ty = y, tz = z, ts = s;
	x = ts * aRef.x + tx * aRef.s + ty * aRef.z - tz * aRef.y;
	y = ts * aRef.y + ty * aRef.s + tz * aRef.x - tx * aRef.z;
	z = ts * aRef.z + tz * aRef.s + tx * aRef.y - ty * aRef.x;
	s = ts * aRef.s - tx * aRef.x - ty * aRef.y - tz * aRef.z;

	Normalize();
}

inline void Quaternion::SetTransformationMatrix(float aMatrix[16]) const
{
	aMatrix[ 0] = 1 - 2 * y * y - 2 * z * z;    aMatrix[ 4] = 2 * x * y - 2 * s * z;        aMatrix[ 8] = 2 * x * z + 2 * s * y;        aMatrix[12] = 0;
	aMatrix[ 1] = 2 * x * y + 2 * s * z;        aMatrix[ 5] = 1 - 2 * x * x - 2 * z * z;    aMatrix[ 9] = 2 * y * z - 2 * s * x;        aMatrix[13] = 0;
	aMatrix[ 2] = 2 * x * z - 2 * s * y;        aMatrix[ 6] = 2 * y * z + 2 * s * x;        aMatrix[10] = 1 - 2 * x * x - 2 * y * y;    aMatrix[14] = 0;
	aMatrix[ 3] = 0;                            aMatrix[ 7] = 0;                            aMatrix[11] = 0;                            aMatrix[15] = 1;
}

inline void Quaternion::SetTransformationMatrix3(float aMatrix[9]) const
{
	aMatrix[0] = 1 - 2 * y * y - 2 * z * z;    aMatrix[3] = 2 * x * y - 2 * s * z;        aMatrix[6] = 2 * x * z + 2 * s * y;
	aMatrix[1] = 2 * x * y + 2 * s * z;        aMatrix[4] = 1 - 2 * x * x - 2 * z * z;    aMatrix[7] = 2 * y * z - 2 * s * x;
	aMatrix[2] = 2 * x * z - 2 * s * y;        aMatrix[5] = 2 * y * z + 2 * s * x;        aMatrix[8] = 1 - 2 * x * x - 2 * y * y;
}

inline void Quaternion::Conjugate(Quaternion& aQuatConj) const
{
	aQuatConj.x = -x;
	aQuatConj.y = -y;
	aQuatConj.z = -z;
	aQuatConj.s =  s;
}

inline void Quaternion::Conjugate() 
{
	x = -x;
	y = -y;
	z = -z;
}

inline void Quaternion::Transform(const Vec3& aIn, Vec3& aOut) const
{
	float tLen = aIn.Magnitude();
	float tLenInv = 1.0f / tLen;

	Quaternion tQuatVect(aIn.X() * tLenInv, aIn.Y() * tLenInv, aIn.Z() * tLenInv, 0.0f);
	Quaternion tQuatTmp;

	Quaternion tConjugate;
	Conjugate(tConjugate);

	tQuatTmp .Mult(*this, tQuatVect);
	tQuatVect.Mult(tQuatTmp, tConjugate);

	aOut.m.x = tQuatVect.x * tLen;
	aOut.m.y = tQuatVect.y * tLen;
	aOut.m.z = tQuatVect.z * tLen;
}

inline void Quaternion::Normalize()
{
	float tLenghtInv = 1.0f / Math::sqrtf(s * s + x * x + y * y + z * z);
	x *= tLenghtInv;
	y *= tLenghtInv;
	z *= tLenghtInv;
	s *= tLenghtInv;
}

inline void Quaternion::GetAxeAndAngle(Vec3& aAxe, float& aAngle)
{
	aAxe.Set(x, y, z);
	float tSin = aAxe.Magnitude();
	aAxe /= tSin;
	aAngle = Math::atan2f(tSin, s);
}

inline void Quaternion::Slerp(const Quaternion& aFrom, const Quaternion& aTo, float aT)
{
	float tProduct = aFrom.s * aTo.s + aFrom.x * aTo.x + aFrom.y * aTo.y + aFrom.z * aTo.z;

	if(ABS(tProduct) < 1.0f)
	{
		// Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
		const float tSign = (tProduct < 0) ? -1.0f : 1.0f;

		const float tTheta =Math::acosf(tSign * tProduct);
		const float s1 = Math::sinf(tSign * aT * tTheta);   
		const float d  = 1.0f / Math::sinf(tTheta);
		const float s0 = Math::sinf((1.0f - aT) * tTheta);

		x = (aFrom.x * s0 + aTo.x * s1) * d;
		y = (aFrom.y * s0 + aTo.y * s1) * d;
		z = (aFrom.z * s0 + aTo.z * s1) * d;
		s = (aFrom.s * s0 + aTo.s * s1) * d;
	}
	else
	{
		*this = aFrom;
	}
}
#endif